
package org.discobots.practice;

import edu.wpi.first.wpilibj.Joystick;

/**
 * @author sam
 */

public class GamePad extends Joystick{
    /**
     * Select the mode with the switch on the back of the joystick.
     * X uses the triggers as an axis but cannot use the vertical Dpad.
     * D allows the vertical Dpad with the triggers as buttons.
    */
    public static final int MODE_X=1, MODE_D=2;
    private int thisMode;
    //For some reason, the base code only supports 6 axes and 12 buttons. Sigh. It's better than normal joysticks.
    private static final int m_numAxes=6;
    private static final int m_numButtons=12;
    //Stores which axes are revered on this gamepad. Note axis k is element k-1
    private double[] reversedAxes=new double[m_numAxes];
    //Declare the axes and buttons.
    public int  AXIS_LX,  AXIS_LY,
                AXIS_RX,  AXIS_RY,
                DPAD_X,   DPAD_Y,
                TRIGGER;
    private int BTN_X,	    BTN_Y,
                BTN_A,	    BTN_B,
		BUMPER_L,   BUMPER_R,
                TRIGGER_L,  TRIGGER_R,
                CLICK_L,    CLICK_R,
		BTN_BACK,   BTN_START;

    /**People will use this constructor...
     *
     * @param port which number joystick is this
     * @param mode what does the switch on the back of the controller say? D is suggested.
     */
    public GamePad(int port, int mode){
        this(port,m_numAxes,m_numButtons);
        switch(mode){
            case MODE_X: {XmodeInit(); thisMode=MODE_X; break;}
            case MODE_D: {DmodeInit(); thisMode=MODE_D; break;}
	    default: {DmodeInit(); thisMode=MODE_D; break;}
        }
    }
    //...and this one is for us to make the joystick stuff work
    protected GamePad(int port, int numAxisTypes, int numButtonTypes){
        super(port,numAxisTypes,numButtonTypes);
	for(int i=0;i<m_numAxes;i++){
	    reversedAxes[i]=1.0;
	}
    }

    /**When the switch on the back of the controller says 'X'
     *
     */
    private void XmodeInit(){
        AXIS_LX=1;  AXIS_LY=2;
        AXIS_RX=4;  AXIS_RY=5;
        DPAD_X=6;   DPAD_Y=0;
        TRIGGER=3;

        BTN_X=3;        BTN_Y=4;
        BTN_A=1;        BTN_B=2;
        BUMPER_L=5;     BUMPER_R=6;
        TRIGGER_L=0;    TRIGGER_R=0;
        CLICK_L=9;      CLICK_R=10;
	BTN_BACK=7;     BTN_START=8;

        setAxisReversed(AXIS_LY,true);
        setAxisReversed(AXIS_RY,true);
        setAxisReversed(TRIGGER,true);
    }

    /**When the switch on the back of the controller says 'D'
     *
     */
    private void DmodeInit(){
        AXIS_LX=1;  AXIS_LY=2;
        AXIS_RX=3;  AXIS_RY=4;
        DPAD_X=5;   DPAD_Y=6;
        TRIGGER=0;

        BTN_X=1;        BTN_Y=4;
        BTN_A=2;        BTN_B=3;
        BUMPER_L=5;     BUMPER_R=6;
        TRIGGER_L=7;    TRIGGER_R=8;
        CLICK_L=11;     CLICK_R=12;
	BTN_BACK=9;     BTN_START=10;

        setAxisReversed(AXIS_LY,true);
        setAxisReversed(AXIS_RY,true);
        setAxisReversed(DPAD_Y,true);
    }

    /**Reverse axes if they are not in the expected direction.
     * This is preconfigured when you choose the mode.
     */
    public void setAxisReversed(int axis, boolean reversed){
	reversedAxes[axis-1]=(reversed ? -1.0 : 1.0);
    }

    public int getNumAxes(){
        return m_numAxes;
    }
    public int getNumButtons(){
        return m_numButtons;
    }
    public int getMode(){
	return thisMode;
    }


    public double getLX(){
        return getRawAxis(AXIS_LX)*reversedAxes[AXIS_LX-1];
    }
    public double getLY(){
        return getRawAxis(AXIS_LY)*reversedAxes[AXIS_LY-1];
    }
    public double getRX(){
        return getRawAxis(AXIS_RX)*reversedAxes[AXIS_RX-1];
    }
    public double getRY(){
        return getRawAxis(AXIS_RY)*reversedAxes[AXIS_RY-1];
    }
    public double getDpadX(){
        return getRawAxis(DPAD_X)*reversedAxes[DPAD_X-1];
    }
    /**Returns 0 if the axis is not supported in your mode.
     *
     */
    public double getDpadY(){
        if(DPAD_Y==0) return 0;
        else return getRawAxis(DPAD_Y)*reversedAxes[DPAD_Y - 1];
    }
    /**Returns 0 if the axis is not supported in your mode.
     *
     */
    public double getTriggerAxis(){
        if(TRIGGER==0) return 0;
        else return getRawAxis(TRIGGER)*reversedAxes[TRIGGER - 1];
    }
}
